#pragma once

#include <pcl/common/transforms.h>
#include "sicp/ICP.h"
#include "registration_interface.hpp"

namespace TESTICP
{
    class SICPRegistration : public RegistrationInterface
    {
    public:
        SICPRegistration(const YAML::Node& node);

        void setInputTarget(const CloudPtr& target) override;
        void setInputSource(const CloudPtr& source) override;
        bool align( CloudType& output_ptr, const Eigen::Matrix4f& predict_pose = Eigen::Matrix4f::Identity()) override;

        inline Eigen::Matrix4f getFinalTransformation() { return final_pose; }
        inline float getFitnessScore() { return 404; }

    private:
        CloudPtr input_target_;
        CloudPtr input_source_;

        Eigen::Matrix4f transformation_;
        Eigen::Matrix4f final_pose;

        SICP::Parameters params_;
    };

    SICPRegistration::SICPRegistration(
        const YAML::Node& node) {
        // parse params:
        params_.p = node["p"].as<double>();
        params_.mu = node["mu"].as<double>();
        params_.alpha = node["alpha"].as<double>();
        params_.max_mu = node["max_mu"].as<double>();
        params_.max_icp = node["max_icp"].as<int>();
        params_.max_outer = node["max_outer"].as<int>();
        params_.max_inner = node["max_inner"].as<int>();
        params_.stop = node["stop"].as<double>();
    }

    void SICPRegistration::setInputTarget(const CloudPtr& target) {
        input_target_ = target;
    }

    void SICPRegistration::setInputSource(const CloudPtr& source) {
        input_source_ = source;
    }

    // 输入为两帧点云，预测的下一帧的位姿，结果位姿
    bool SICPRegistration::align( CloudType& output_ptr, const Eigen::Matrix4f& predict_pose) {

        // pre-process input source:
        CloudPtr transformed_input_source(new CloudType());
        pcl::transformPointCloud(*input_source_, *transformed_input_source, predict_pose);

        // TODO: second option -- adapt existing implementation
        //
        // TODO: format inputs for SICP:

        Eigen::Matrix3Xd source(3, transformed_input_source->size());
        Eigen::Matrix3Xd target(3, input_target_->size());

        for (int i = 0; i < transformed_input_source->size(); i++) {
            source(0, i) = transformed_input_source->points[i].x;
            source(1, i) = transformed_input_source->points[i].y;
            source(2, i) = transformed_input_source->points[i].z;
        }

        for (int i = 0; i < input_target_->size(); i++) {
            target(0, i) = input_target_->points[i].x;
            target(1, i) = input_target_->points[i].y;
            target(2, i) = input_target_->points[i].z;
        }
        // TODO: SICP registration:
        transformation_.setZero();

        Eigen::Affine3d result = SICP::point_to_point(source, target, params_);

        transformation_ = result.matrix().cast<float>();

        // set output:
        final_pose = transformation_ * predict_pose;
        pcl::transformPointCloud(*input_source_, output_ptr, final_pose);

        // 这里需要更新上一帧的点云
        // SetInputTarget(output_ptr);

        return true;
    }
}
